/*
 * nav_recip_rca_test.cpp
 * nav_recip_rca
 *
 *
 * Copyright (c) 2008-2012, University of North Carolina at Chapel Hill
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *   * Neither the name of the University of North Carolina at Chapel Hill nor
 *     the names of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 *
 * Please send all bug reports to <geom@cs.unc.edu>.
 *
 * The authors may be contacted via:
 *
 *     Dept. of Computer Science
 *     201 S. Columbia St.
 *     Frederick P. Brooks, Jr. Computer Science Bldg.
 *     Chapel Hill, N.C. 27599-3175
 *     United States of America
 *
 *     http://gamma.cs.unc.edu/ROS/
 */

#include <utility>
#include <vector>
#include <gtest/gtest.h>
#include <nav_recip_rca/nav_recip_rca.h>

namespace nav_recip
{
// Test fixture for Line and line.h.
class LineTest : public ::testing::Test { };

// Test fixture for RCARobot and rca_robot.h.
class RCARobotTest : public ::testing::Test { };

// Test constructors in Line.
TEST_F(LineTest, Constructor)
{
  // Default constructor.
  const Line line1;

  EXPECT_FLOAT_EQ(0.0f, line1.position_.getX());
  EXPECT_FLOAT_EQ(0.0f, line1.position_.getY());
  EXPECT_FLOAT_EQ(0.0f, line1.direction_.getX());
  EXPECT_FLOAT_EQ(0.0f, line1.direction_.getY());

  // Constructor given position of a point on the Line and direction.
  const Line line2(Vector2(1.0f, 2.0f), Vector2(3.0f, 4.0f));

  EXPECT_FLOAT_EQ(1.0f, line2.position_.getX());
  EXPECT_FLOAT_EQ(2.0f, line2.position_.getY());
  EXPECT_FLOAT_EQ(3.0f, line2.direction_.getX());
  EXPECT_FLOAT_EQ(4.0f, line2.direction_.getY());

  // Copy constructor.
  const Line line3(Vector2(5.0f, 6.0f), Vector2(7.0f, 8.0f));
  const Line line4(line3);

  EXPECT_FLOAT_EQ(5.0f, line4.position_.getX());
  EXPECT_FLOAT_EQ(6.0f, line4.position_.getY());
  EXPECT_FLOAT_EQ(7.0f, line4.direction_.getX());
  EXPECT_FLOAT_EQ(8.0f, line4.direction_.getY());
}

// Test getters in Line.
TEST_F(LineTest, Get)
{
  const Line line(Vector2(2.0f, 4.0f), Vector2(6.0f, 8.0f));

  const Vector2& position = line.getPosition();
  const Vector2& direction = line.getDirection();

  EXPECT_FLOAT_EQ(2.0f, position.getX());
  EXPECT_FLOAT_EQ(4.0f, position.getY());
  EXPECT_FLOAT_EQ(6.0f, direction.getX());
  EXPECT_FLOAT_EQ(8.0f, direction.getY());
}

// Test setters in Line.
TEST_F(LineTest, Set)
{
  Line line;

  line.setPosition(Vector2(3.0f, 6.0f));
  line.setDirection(Vector2(9.0f, 12.0f));

  EXPECT_FLOAT_EQ(3.0f, line.getPosition().getX());
  EXPECT_FLOAT_EQ(6.0f, line.getPosition().getY());
  EXPECT_FLOAT_EQ(9.0f, line.getDirection().getX());
  EXPECT_FLOAT_EQ(12.0f, line.getDirection().getY());
}

// Test constructors in RCARobot.
TEST_F(RCARobotTest, Constructor)
{
  // Default constructor.
  const RCARobot robot1;

  EXPECT_FLOAT_EQ(0.0f, robot1.radius_);
  EXPECT_FLOAT_EQ(0.0f, robot1.position_.getX());
  EXPECT_FLOAT_EQ(0.0f, robot1.position_.getY());
  EXPECT_FLOAT_EQ(0.0f, robot1.velocity_.getX());
  EXPECT_FLOAT_EQ(0.0f, robot1.velocity_.getY());
  EXPECT_FLOAT_EQ(0.0f, robot1.preferred_velocity_.getX());
  EXPECT_FLOAT_EQ(0.0f, robot1.preferred_velocity_.getY());
  EXPECT_FLOAT_EQ(0.0f, robot1.max_speed_);
  EXPECT_FLOAT_EQ(0.0f, robot1.time_horizon_);
  EXPECT_FLOAT_EQ(0.0f, robot1.new_velocity_.getX());
  EXPECT_FLOAT_EQ(0.0f, robot1.new_velocity_.getY());
  EXPECT_TRUE(robot1.lines_.empty());
  EXPECT_TRUE(robot1.neighbors_.empty());

  // Constructor given bounding radius, current position, current linear
  // velocity, preferred linear velocity, maximum linear speed, and time
  // horizon.
  const RCARobot robot2(1.0f, Vector2(2.0f, 3.0f), Vector2(4.0f, 5.0f), Vector2(6.0f, 7.0f), 8.0f, 9.0f);

  EXPECT_FLOAT_EQ(1.0f, robot2.radius_);
  EXPECT_FLOAT_EQ(2.0f, robot2.position_.getX());
  EXPECT_FLOAT_EQ(3.0f, robot2.position_.getY());
  EXPECT_FLOAT_EQ(4.0f, robot2.velocity_.getX());
  EXPECT_FLOAT_EQ(5.0f, robot2.velocity_.getY());
  EXPECT_FLOAT_EQ(6.0f, robot2.preferred_velocity_.getX());
  EXPECT_FLOAT_EQ(7.0f, robot2.preferred_velocity_.getY());
  EXPECT_FLOAT_EQ(8.0f, robot2.max_speed_);
  EXPECT_FLOAT_EQ(9.0f, robot2.time_horizon_);
  EXPECT_FLOAT_EQ(0.0f, robot2.new_velocity_.getX());
  EXPECT_FLOAT_EQ(0.0f, robot2.new_velocity_.getY());
  EXPECT_TRUE(robot2.lines_.empty());
  EXPECT_TRUE(robot2.neighbors_.empty());
}

// Test getters in RCARobot.
TEST_F(RCARobotTest, Get)
{
  RCARobot robot(2.0f, Vector2(4.0f, 6.0f), Vector2(8.0f, 10.0f), Vector2(12.0f, 14.0f), 16.0f, 18.0f);

  robot.new_velocity_ = Vector2(20.0f, 22.0f);
  robot.lines_.push_back(Line(Vector2(24.0f, 26.0f), Vector2(28.0f, 30.0f)));

  Robot neighbor(32.0f, Vector2(34.0f, 36.0f), Vector2(38.0f, 40.0f));
  robot.neighbors_.push_back(&neighbor);

  const float radius = robot.getRadius();
  const Vector2& position = robot.getPosition();
  const Vector2& velocity = robot.getVelocity();
  const Vector2& preferred_velocity = robot.getPreferredVelocity();
  const float max_speed = robot.getMaxSpeed();
  const float time_horizon = robot.getTimeHorizon();
  const Vector2& new_velocity = robot.getNewVelocity();
  const LineList lines = robot.getLines();
  const ConstRobotList& neighbors = robot.getNeighbors();

  EXPECT_FLOAT_EQ(2.0f, radius);
  EXPECT_FLOAT_EQ(4.0f, position.getX());
  EXPECT_FLOAT_EQ(6.0f, position.getY());
  EXPECT_FLOAT_EQ(8.0f, velocity.getX());
  EXPECT_FLOAT_EQ(10.0f, velocity.getY());
  EXPECT_FLOAT_EQ(12.0f, preferred_velocity.getX());
  EXPECT_FLOAT_EQ(14.0f, preferred_velocity.getY());
  EXPECT_FLOAT_EQ(16.0f, max_speed);
  EXPECT_FLOAT_EQ(18.0f, time_horizon);
  EXPECT_FLOAT_EQ(20.0f, new_velocity.getX());
  EXPECT_FLOAT_EQ(22.0f, new_velocity.getY());
  EXPECT_EQ(1, lines.size());
  EXPECT_FLOAT_EQ(24.0f, lines[0].position_.getX());
  EXPECT_EQ(1, neighbors.size());
  EXPECT_FLOAT_EQ(32.0f, neighbors[0]->radius_);
}

// Test setters in RCARobot.
TEST_F(RCARobotTest, Set)
{
  RCARobot robot;

  robot.setRadius(3.0f);
  robot.setPosition(Vector2(6.0, 9.0f));
  robot.setVelocity(Vector2(12.0, 15.0f));
  robot.setPreferredVelocity(Vector2(18.0, 21.0f));
  robot.setMaxSpeed(24.0f);
  robot.setTimeHorizon(27.0f);

  ConstRobotList neighbors;
  Robot neighbor(30.0f, Vector2(33.0f, 36.0f), Vector2(39.0f, 42.0f));
  neighbors.push_back(&neighbor);
  robot.setNeighbors(neighbors);

  EXPECT_FLOAT_EQ(3.0f, robot.getRadius());
  EXPECT_FLOAT_EQ(6.0f, robot.getPosition().getX());
  EXPECT_FLOAT_EQ(9.0f, robot.getPosition().getY());
  EXPECT_FLOAT_EQ(12.0f, robot.getVelocity().getX());
  EXPECT_FLOAT_EQ(15.0f, robot.getVelocity().getY());
  EXPECT_FLOAT_EQ(18.0f, robot.getPreferredVelocity().getX());
  EXPECT_FLOAT_EQ(21.0f, robot.getPreferredVelocity().getY());
  EXPECT_FLOAT_EQ(24.0f, robot.getMaxSpeed());
  EXPECT_FLOAT_EQ(27.0f, robot.getTimeHorizon());
  EXPECT_FLOAT_EQ(0.0f, robot.getNewVelocity().getX());
  EXPECT_FLOAT_EQ(0.0f, robot.getNewVelocity().getY());
  EXPECT_TRUE(robot.getLines().empty());
  EXPECT_EQ(1, robot.getNeighbors().size());
  EXPECT_FLOAT_EQ(30.0f, robot.getNeighbors()[0]->getRadius());
}

// Test computeNewVelocity() in RCARobot.
TEST_F(RCARobotTest, ComputeNewVelocity)
{
  // Lists and iterators of pairs of Robot pointers and Vector2 objects.
  typedef std::vector<std::pair<Robot*, Vector2> > RobotGoalList;
  typedef std::vector<std::pair<Robot*, Vector2> >::iterator RobotGoalIterator;
  typedef std::vector<std::pair<Robot*, Vector2> >::const_iterator RobotGoalConstIterator;

  RobotGoalList robots_goals;
  float global_time = 0.0f;

  // Predefined start and goal positions on the perimeter of a circular
  // environment.
  Vector2 start_positions[16] = { Vector2(31.712, -0.413), Vector2(29.989, 11.931), Vector2(22.369, 22.571),
      Vector2(11.954, 29.174), Vector2(-0.239, 32.177), Vector2(-12.161, 29.685), Vector2(-22.397, 22.304),
      Vector2(-29.554, 12.397), Vector2(-31.751, 0.335), Vector2(-29.886,	-12.228), Vector2(-22.949, -22.769),
      Vector2(-12.341, -30.032), Vector2(-0.019, -31.752), Vector2(11.853, -29.994), Vector2(22.132, -22.246),
      Vector2(29.771, -12.517) };

  Vector2 goal_positions[16] = { Vector2(-31.785, 0.0191), Vector2(-29.351, -12.181), Vector2(-22.822, -22.408),
      Vector2(-12.722, -29.808), Vector2(0.103, -32.389), Vector2(12.139,	-29.581), Vector2(22.348,	-23.032),
      Vector2(29.241, -12.135), Vector2(32.498, -0.188), Vector2(29.513, 12.295), Vector2(22.919, 22.904),
      Vector2(12.713, 29.524), Vector2(0.036, 31.589), Vector2(-11.926, 29.209), Vector2(-22.363, 22.803),
      Vector2(-29.771, 12.689) };

  const float time_step = 0.25f;

  // Add robots and goals to the test.
  for (size_t i = 0; i < 16; ++i) {
    Robot* robot = new RCARobot(1.0f, start_positions[i], Vector2(), Vector2(), 3.0f, 10.0f);
    const Vector2& goal = goal_positions[i];

    std::pair<Robot*, Vector2> robot_goal = std::make_pair(robot, goal);

    robots_goals.push_back(robot_goal);
  }

  ASSERT_EQ(16, robots_goals.size());

  // Add neighbors of each RCARobot.
  for (RobotGoalIterator it1 = robots_goals.begin(); it1 != robots_goals.end(); ++it1) {
    Robot* robot1 = (*it1).first;
    std::vector<const Robot*> neighbors;

    for (RobotGoalConstIterator it2 = robots_goals.begin(); it2 != robots_goals.end(); ++it2) {
      const Robot* robot2 = (*it2).first;

      if (robot1 != robot2) {
        neighbors.push_back(robot2);
      }
    }
    
    robot1->setNeighbors(neighbors);

    ASSERT_EQ(15, robot1->getNeighbors().size());
  }

  // Run the test until all robots have reached their goals.
  bool test_completed = false;

  ASSERT_FLOAT_EQ(0.0f, global_time);
  ASSERT_GT(time_step, 0.0f);

  while (!test_completed) {
    // Check whether each RCARobot has reached its goal.
    test_completed = true;

    for (RobotGoalConstIterator it = robots_goals.begin(); it != robots_goals.end(); ++it) {
      Robot* robot = (*it).first;
      const Vector2& goal = (*it).second;

      const float distance_squared = robot->getPosition().distance2(goal);

      if (distance_squared > robot->getRadius() * robot->getRadius()) {
        test_completed = false;

        break;
      }
    }

    // Compute preferred linear velocity, then new linear velocity of each
    // RCARobot.
    for (RobotGoalIterator it = robots_goals.begin(); it != robots_goals.end(); ++it) {
      Robot* robot = (*it).first;
      const Vector2& goal = (*it).second;

      // Compute preferred linear velocity.
      robot->setPreferredVelocity(goal - robot->getPosition());
      const float speed = robot->getPreferredVelocity().length();

      if (speed > 1.0f) {
        robot->preferred_velocity_ /= speed;
      }

      // Compute new linear velocity.
      robot->computeNewVelocity(time_step);
    }

    // Update current position and current linear velocity of each RCARobot.
    for (RobotGoalIterator it = robots_goals.begin(); it != robots_goals.end(); ++it) {
      Robot* robot = (*it).first;

      robot->setVelocity(robot->getNewVelocity());
      robot->setPosition(robot->getPosition() + time_step * robot->getVelocity());
    }

    // Update global time.
    global_time += time_step;
  }

  EXPECT_GT(global_time, 10.0f);

  std::cout << "[==========]" << std::endl;
  std::cout << "[ SIM TIME ] " << global_time << " s" << std::endl;
  std::cout << "[==========]" << std::endl;

  SUCCEED();
}
}

// Run all unit tests for this package.
int main(int argc, char* argv[])
{
  testing::InitGoogleTest(&argc, argv);

  return RUN_ALL_TESTS();
}
